#include "laplace.h"

LaplaceOperator::LaplaceOperator(CGMesh& m) : mesh(m)
{
    //create sparse matrices
    weights = Eigen::DynamicSparseMatrix< float >( mesh.vert.size(), mesh.vert.size() );
    areas = Eigen::DynamicSparseMatrix< float >( mesh.vert.size(), mesh.vert.size() );


    //for each face
    for ( CGMesh::FaceIterator fit = mesh.face.begin(); fit != mesh.face.end(); ++fit ) {

        int findex = fit - mesh.face.begin();
        //for each edge, we have to recover the indices of the vertices
        for ( int i = 0; i < 3; i ++ ) {
            int index0 = vcg::tri::Index( mesh, (*fit).V(i)  );
            int index1 = vcg::tri::Index( mesh, (*fit).V1(i) );
            //sanity test
            //assert ( weights.coeffRef(index0, index1) == 0.0f );

            //compute weight of edge i and set it in the weight matrix
            weights.coeffRef(index0, index1) = -getWeight( findex, i );
            //update weights of diagonal
            weights.coeffRef(index0, index0) -= weights.coeffRef(index0, index1);

            //if is on border, we should set also the (index1, index0) element of the matrix
            if ( vcg::face::IsBorder<CGFace>( mesh.face[findex], i )  ) {
                weights.coeffRef(index1, index0) = -weights.coeffRef(index0, index1);
                //update weights of diagonal
                weights.coeffRef(index0, index0) -= weights.coeffRef(index1, index0);
            }

            //compute area
            areas.coeffRef(index0, index0) += getMixedArea( findex, i );

        }

    }
    //normalize diagonal
    /*for ( int k = 0; k < weights.outerSize(); k ++ ) {
        double diag = -weights.coeffRef(k, k);

        for ( Eigen::DynamicSparseMatrix<float>::InnerIterator it(weights, k); it; ++it ) {
            weights.coeffRef(it.row(), it.col()) = it.value() / diag;
        }
    }*/

    ///TODO: un check sulla differenza delle somme?

    //invert areas
    for ( int j=0; j < mesh.vert.size(); j ++ )
        areas.coeffRef(j, j) = 1.0 / (2.0 * areas.coeffRef(j, j));


    std::fstream matrixW("../output/matrixW.txt", std::ios::out);

    for ( int k = 0; k < weights.outerSize(); k ++ ) {

        for ( Eigen::DynamicSparseMatrix<float>::InnerIterator it(weights, k); it; ++it ) {
            matrixW << it.row() << " " << it.col() << " " << it.value() << std::endl;
        }
    }

    matrixW.close();
}

void LaplaceOperator::LaplaceSmoothing(int iter)
{

    Eigen::DynamicSparseMatrix< float > tmp;
    tmp = weights;

    for ( int i = 0; i < iter; i ++ ) {

        //create a Nx3 vector containing coordinates
        Eigen::MatrixXf coords( mesh.vert.size(), 3 );

        //insert data
        for ( int j=0; j < mesh.vert.size(); j ++ ) {
            coords(j, 0) = mesh.vert[j].P().X();
            coords(j, 1) = mesh.vert[j].P().Y();
            coords(j, 2) = mesh.vert[j].P().Z();
            if ( j == 0 )
                qDebug() << "Before " <<  mesh.vert[j].P().X() << " " << mesh.vert[j].P().Y() << " " << mesh.vert[j].P().Z();
        }

        //apply laplacian
        Eigen::MatrixXf newcoords = coords + (weights * coords);

        for ( int j=0; j < mesh.vert.size(); j ++ ) {
            mesh.vert[j].P().X() = newcoords(j, 0);
            mesh.vert[j].P().Y() = newcoords(j, 1);
            mesh.vert[j].P().Z() = newcoords(j, 2);

            if ( j == 0 )
                qDebug() << "After " <<  mesh.vert[j].P().X() << " " << mesh.vert[j].P().Y() << " " << mesh.vert[j].P().Z();

        }
    }

    vcg::tri::io::ExporterPLY<CGMesh>::Save(mesh, "../output/roba.ply");
}


void LaplaceOperator::LaplaceSmoothingDirect(int iter, QString type)
{

    std::vector< CGPoint > newpoints( mesh.vert.size() );
    std::vector< double > area( mesh.vert.size() );

    double avgedge = 0.0;

    for ( int k = 0; k < iter; k ++ ) {
        //unmark all
        vcg::tri::UnMarkAll( mesh );

        //for each face
        for ( int i = 0; i < mesh.face.size(); i ++ ) {
            //for each vertex
            for ( int j = 0; j < 3; j ++ ) {
                //marked, skip
                avgedge += ( mesh.face[i].P(j) - mesh.face[i].P1(j)).Norm();
                if ( vcg::tri::IsMarked(mesh, mesh.face[i].V(j)) ) continue;

                vcg::tri::Mark(mesh,  mesh.face[i].V(j));

                //not marked: create pos
                CGMesh::VertexPointer v = mesh.face[i].V(j);
                CGMesh::FacePointer start = &(mesh.face[i]);
                vcg::face::Pos<CGFace> pos(start, j, v);

                int index = vcg::tri::Index( mesh, v );

                newpoints[index] = CGPoint(0,0,0);
                area[index] = 0.0;
                int c = 0;

                //1-ring
                do {
                    if ( type == QString("uniform") )
                        newpoints[index] += (pos.F()->P1(pos.E()) - mesh.vert[index].P()) * 1.0f;
                    else
                        newpoints[index] += (pos.F()->P1(pos.E()) - mesh.vert[index].P()) * getWeight( vcg::tri::Index(mesh, pos.F()), pos.E() );

                    area[index]  += getMixedArea( vcg::tri::Index(mesh, pos.F()), pos.VInd() );
                    c++;
                    pos.FlipF();
                    pos.FlipE();
                }
                while(pos.F() != start);

                //laplace
                if ( type == QString("uniform") )  newpoints[index] = newpoints[index] / c;
            }

        }

        avgedge /= (2.0 * mesh.face.size());

        float delta = 0.2f;
        //double minarea = *(std::min_element( area.begin(), area.end() ));

        double avgarea = 0.0f;
        for ( int i = 0; i < mesh.vert.size(); i ++ )
            avgarea += area[i];

        avgarea /= mesh.vert.size();

        //normalize areas (HOW)
        for ( int i = 0; i < mesh.vert.size(); i ++ )
            area[i] = area[i]/avgarea;

        for ( int i = 0; i < mesh.vert.size(); i ++ )
            mesh.vert[i].P() += newpoints[i] * (1.0f / (2.0f)) * delta;
    }

    vcg::tri::io::ExporterPLY<CGMesh>::Save(mesh, "../output/roba.ply");
}


float LaplaceOperator::getWeight( int fp, int ej )
{
    //compute the weight associated with edge ej of face fp
    //weight is computed using cotangent

    CGMesh::FacePointer current = &mesh.face[fp];
    CGMesh::FacePointer opposite = current->FFp(ej);
    int eo = current->FFi(ej);

    //alpha angle (current face, opposite to edge ej)
    float alpha = vcg::Angle( (current->P(ej)  - current->P2(ej)).Normalize(), (current->P1(ej) - current->P2(ej)).Normalize() );
    //beta angle (opposite face, opposite to eo)
    float beta  = vcg::Angle( (opposite->P(eo) - opposite->P2(eo)).Normalize(), (opposite->P1(eo) - opposite->P2(eo)).Normalize() );

    //if it's border...
    /*if ( vcg::face::IsBorder<CGFace>(*current, ej) ) {
        //...then we use only the cotangent of alpha
        return ( cos(alpha)/sin(alpha) );
    }*/

    //if it's not border, use half-negative cotangent weight
    float w = (cos(alpha)/sin(alpha)) + (cos(beta)/sin(beta));
    return w / 2.0f;
}


float LaplaceOperator::getMixedArea( int fp, int vj )
{

    return vcg::DoubleArea( mesh.face[fp] ) / 6.0;

    float angles[3];
    CGMesh::FacePointer current = &mesh.face[fp];

    //compute the angle of fp incident to vj
    for (int i = 0; i < 3; i ++) angles[i] = vcg::Angle( current->P(i) - current->P2(i), current->P1(i) - current->P2(i)  );

    //now search for obtuse angles
    bool obtuse = false;

    for ( int i = 0; i < 3; i ++ )
        if ( cos(angles[i])/sin(angles[i]) < 0 ) obtuse = true;

    //we found an obtuse angle
    if ( obtuse ) {
        //the obtuse angle is in vj
        if ( cos(angles[vj])/sin(angles[vj]) < 0 )
            return vcg::DoubleArea( *current ) / 4.0;
        else //not in vj
            return vcg::DoubleArea( *current ) / 8.0;
    }

    //no obtuse angle: compute and return the voronoi area
    CGPoint p0 = current->P(vj);
    CGPoint p1 = vcg::Circumcenter( *current );

    CGPoint p2 = (current->P(vj) + current->P1(vj)) / 2.0;
    CGPoint p3 = (current->P(vj) + current->P2(vj)) / 2.0;

    return (p1-p0).Norm() * (p3-p2).Norm() / 2.0;
}
